/*
 * FormationSquare.java
 *
 * Created on 18 de noviembre de 2007, 07:04 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Behaviors.Formations;


import Utils.Vector2d;
/**
 *
 * @author alejo
 */
public class FormationColumn extends Formation {
    
    
    /** Creates a new instance of FormationSquare */
    public FormationColumn(int numRobots) {
        super(numRobots);
         
        this.setName("COLUMN");
        setWidth(0.5f); //width of form depend on width of robot
    }
    
    public Vector2d getRobotPosition(int robotId){
        if(robotId>=0 && robotId<this.getNumRobots()){
            
            //get angle of location in Formation
            float theta = (float)getRobotLocationDir(robotId);
            
            
                
            return getVectorInForm(theta,robotId);
            
        }
            
        return null;
    
    }
    
    
    public Vector2d getVectorInForm(float theta, int robotId){
        
        
        
        float distance = 1;
        float x = 0;
        float y = 0;

        /*
        switch(robotId){
            
            case 0:
                x = 1.0f/3.0f;
            break;
            case 1:
                x = -1.0f/3.0f;
            break;
            case 2:
                x = -1;
            break;
            case 3:
                x = 1;
            break;
            
        }
        */
        if(this.getNumRobots() % 2 == 0)
        {
                      
            if(robotId == this.getNumRobots()/4 -1)
                x = distance/2;
            else if(robotId == this.getNumRobots()/4)
                x = -distance/2;
            else if(theta <= Math.PI / 2+0.01) // ok
                x = -(float)(distance  *((((this.getNumRobots()/4)-robotId+1)*2)+1 - 0.5));
                
            else if (theta <= Math.PI)
                y =(float)(distance  *(((robotId - this.getNumRobots()/4)*2+1 - 0.5)));
            else if (theta <= Math.PI + Math.PI / 2)
                x =-(float)(distance  *(((((this.getNumRobots()*3/4-1))-robotId+1)*2) - 0.5));
            else
                
                x = distance  * (float)(((this.getNumRobots()*3/4)-robotId+1)*2 - 0.5);
        }
        else
        {
            
            if(robotId == 0)
                x = 0;
            else if(theta <= Math.PI / 2+0.01) // ok
                x = distance  * (float)((robotId)*2);
            else if (theta <= Math.PI)
                x = (float)(distance  *(((this.getNumRobots()-1)/2 -robotId)*2+1));
            else if (theta <= Math.PI + Math.PI / 2)
                x =-(float)(distance  *(((robotId - ((this.getNumRobots()-1)/2+1))*2)+1));
            else
                x = -(float)(distance  *((((this.getNumRobots() - 1)-robotId)*2)+2));
             
            
        }
        
        y = 0;
        /*
        System.out.print("Angle: ");
        System.out.print(theta);
        System.out.print( "Angle var: ");
        System.out.print(this.getThetaVar());
        System.out.print(" Distance: ");
        
        System.out.println(distance);
        
        
        System.out.print("Robot Id: ");
        System.out.print(robotId);
        System.out.print(" x: ");
        System.out.print(x);
        System.out.print(" y: ");
        System.out.println(y);
        */
        
        
        
        
        if(robotId>=this.getNumRobots())
            return null;
        else
            return new Vector2d(x,y,Vector2d.MODE_XY);
        
    
    }
    
    public void calculateScaleFactor()
    {
        scaleFactor = (float)(width/2);
    }
    
    

   
    
}
